//
// Created by nuo on 2021/11/18.
//


#include "serial_test/serial_test.h"


namespace serial_test{

    SerialTest::SerialTest(ros::NodeHandle &nh) {
        //测试小案例
        write_sub = nh.subscribe<std_msgs::String>("write", 1000, &SerialTest::write_callback,
                                                              this);
        read_pub = nh.advertise<std_msgs::String>("read",1000);

        //解析cmd_vel控制指令，并把指令下发给单片机
        cmd_vel_sub = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 1000, &SerialTest::cmd_twist_callback,
                                                   this);
        //从单片机获得数据，进行有效数据的截取（功能待完善）
        odom_stc_sub = nh.subscribe<std_msgs::Float32MultiArray>("/stc_sub", 1000, &SerialTest::stc_sub_callback,
                                                         this);
        //把从单片机获得的数据进行封装，消息类型为：nav_msgs::Odometry，话题名为：/odom_stc
        odom_stc_pub = nh.advertise<nav_msgs::Odometry>("/odom_stc",1000);

        config_node_ = YAML::LoadFile(SERIAL_SPACE_PATH);
        allocateMemory();
    }

    void SerialTest::write_callback(const std_msgs::String_<std::allocator<void>>::ConstPtr &msg) {
        ROS_INFO_STREAM("Write to serial port" << msg->data);
        ser.write(msg->data);
    }

    //工控机下发单片机控制指令： /cmd_vel
    void SerialTest::cmd_twist_callback(const geometry_msgs::TwistConstPtr &cmd_twist_msg) {
        geometry_msgs::Twist cmd_twist;
        cmd_twist = *cmd_twist_msg;
        ROS_INFO("velocity_x = %f, velocity_y = %f, velocity_v = %f",cmd_twist.linear.x
                                                                    ,cmd_twist.linear.y
                                                                    ,cmd_twist.angular.z);
        velocity_x.fvalue = cmd_twist.linear.x;
        velocity_y.fvalue = cmd_twist.linear.y;
        angular_v.fvalue = cmd_twist.angular.z;
        //float --> bite
        float2bytes(float (velocity_x.fvalue),velocity_x.cvalue);
        float2bytes(float (velocity_y.fvalue),velocity_y.cvalue);
        float2bytes(float (angular_v.fvalue) ,angular_v.cvalue);

        std::uint8_t cmd_twist_buffer[num_byte_to_stc];  // cmd_vel上的数据转换成byte数据的存储容器
        memset(cmd_twist_buffer,0,sizeof(cmd_twist_buffer));		//内存空间初始化为0

        //数据打包（1.包头 2.地址（可省略）3.包长度 4.有效数据 5.数据校验 6.包尾）
        //1.//数据帧头（包头），数据内容暂定
        cmd_twist_buffer[0] = 0xff;
        cmd_twist_buffer[1] = 0xfe;
        //2.地址，省略
        //3.包长度，通过数组创建的对象，包长度可通过数组的大小确定
        //4.有效数据（【2】- 【13】）
        cmd_twist_buffer[2] = velocity_x.cvalue[0];
        cmd_twist_buffer[3] = velocity_x.cvalue[1];
        cmd_twist_buffer[4] = velocity_x.cvalue[2];
        cmd_twist_buffer[5] = velocity_x.cvalue[3];

        cmd_twist_buffer[6] = velocity_y.cvalue[0];
        cmd_twist_buffer[7] = velocity_y.cvalue[1];
        cmd_twist_buffer[8] = velocity_y.cvalue[2];
        cmd_twist_buffer[9] = velocity_y.cvalue[3];

        cmd_twist_buffer[10] = angular_v.cvalue[0];
        cmd_twist_buffer[11] = angular_v.cvalue[1];
        cmd_twist_buffer[12] = angular_v.cvalue[2];
        cmd_twist_buffer[13] = angular_v.cvalue[3];
        //5.数据校验
        //CRC 校验和从有效数据开始取异或 （奇偶校验）
        cmd_twist_buffer[14] = cmd_twist_buffer[2]^cmd_twist_buffer[3]^cmd_twist_buffer[4]^cmd_twist_buffer[5]^cmd_twist_buffer[6]^cmd_twist_buffer[7]^
                       cmd_twist_buffer[8]^cmd_twist_buffer[9]^cmd_twist_buffer[10]^cmd_twist_buffer[11]^cmd_twist_buffer[12]^cmd_twist_buffer[13];
        //6.包尾 数据内容暂定
        cmd_twist_buffer[15] = 0xfe;
        cmd_twist_buffer[16] = 0xff;

        //通过串口发送给单片机
        ser.write(cmd_twist_buffer,num_byte_to_stc);

        ROS_INFO("data send successful");
    }

    // 有效数据的抓取
    // 待完成..........
    void SerialTest::stc_sub_callback(const std_msgs::Float32MultiArrayConstPtr &std_sub_msg) {
        int msg_num = std_sub_msg->data.size();
        if (msg_num < num_byte_from_stc) return;  //num_byte_from_stc = 29

    }

    bool SerialTest::serial_opened() {
        //检测串口是否已经打开，并给出提示信息
        if(ser.isOpen()){
            ROS_INFO_STREAM("Serial Port initialized");
            return true;
        }
        return false;
    }
    void SerialTest::read_test() {

        if (ser.available()){
            ROS_INFO_STREAM("Reading from serial port\n");
            std_msgs::String result;
            result.data = ser.read(ser.available());
            ROS_INFO_STREAM("Read: " << result.data);
            read_pub.publish(result);
        }
    }

    void SerialTest::send_odom_stc() {

        //假设一个完整的数据有 29 bytes,2个head,2个end,1个CRC校验，6个float类型（6*4 = 24 bytes）
        //内容暂定
        std::uint8_t odom_stc_buffer[num_byte_from_stc];   // 从stc单片机上获得的byte数据的存储容器
        memset(odom_stc_buffer,0,sizeof(odom_stc_buffer));		//内存空间初始化为0
        //odom_stc_buffer = *********    //odom_stc_buffer = 某个数
        // 6 个数据分别为：（0）position_x, （1）position_y, （2）linear_x, （3）linear_y, （4）angular_speed_z, （5）angular_pose_z
        for (int i = 0; i < 6; ++i) {    // i 表示有6个数据
            for (int j = 0; j < 4; ++j) {    // j 表示每个数据用4个byte
                odom_stc_array[i].cvalue[j] = odom_stc_buffer[i * 4 + 2];    // 2表示数据包头有2个byte
            }
            odom_stc_array[i].fvalue = bytes2float(odom_stc_array[i].cvalue);  //数据转换成float
        }
        odom_stc_data.header.stamp = ros::Time::now();
        odom_stc_data.header.frame_id = "/map";
        odom_stc_data.pose.pose.position.x = odom_stc_array[0].fvalue;
        odom_stc_data.pose.pose.position.y = odom_stc_array[1].fvalue;
        Eigen::Quaternionf q_odom_ = Euler2Quaternionf(float(odom_stc_array[5].fvalue), 0, 0);
        odom_stc_data.pose.pose.orientation.x = q_odom_.x();
        odom_stc_data.pose.pose.orientation.y = q_odom_.y();
        odom_stc_data.pose.pose.orientation.z = q_odom_.z();
        odom_stc_data.pose.pose.orientation.w = q_odom_.w();
        odom_stc_data.twist.twist.linear.x = odom_stc_array[2].fvalue;
        odom_stc_data.twist.twist.linear.y = odom_stc_array[3].fvalue;
        odom_stc_data.twist.twist.angular.z = odom_stc_array[4].fvalue;

        odom_stc_pub.publish(odom_stc_data);
    }


    void SerialTest::allocateMemory() {
        try{
            ser.setPort("/dev/ttyUSB0");  //串口号待定
            ser.setBaudrate(115200);
            serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
            ser.setTimeout(timeout);
            ser.open();
        }
        catch (serial::IOException& e){
            ROS_ERROR_STREAM("Unable to open port");
            return;
        }

        num_byte_to_stc = config_node_["num_byte_to_stc"].as<int>();
        num_byte_from_stc = config_node_["num_byte_from_stc"].as<int>();

    }

    void SerialTest::float2bytes(float p, unsigned char *bytes) {
        // unsigned char *pchar = (unsigned char*)&p;
        auto *pchar = (unsigned char*)&p;
        for(int i=0;i < sizeof(float);i++)
        {
            *bytes = *pchar;
            pchar++;
            bytes++;
        }
    }
    float SerialTest::bytes2float(const unsigned char *bytes) {
        return *((float*)bytes);//强制转换
    }
    Eigen::Quaternionf SerialTest::Euler2Quaternionf(float eluer_z, float eluer_y, float eluer_x) {
        Eigen::Quaternionf q = Eigen::AngleAxisf (eluer_z, ::Eigen::Vector3f::UnitZ()) *
                               Eigen::AngleAxisf(eluer_y, ::Eigen::Vector3f::UnitY()) *
                               Eigen::AngleAxisf(eluer_x, ::Eigen::Vector3f::UnitX());
        return q;
    }


    void SerialTest::Run(){

        if (serial_opened()){

            read_test();  //测试串口通信

            send_odom_stc();  //发布从stc获得的数据，封装成 /odom_stc 发布出去
        }

    }

}